
import time 
import math
from threading import Thread 

from Brain.actuators.wheels import Wheels
from Brain.sensors.encoder import Encoder
from Brain.sensors.encoder_calculation import EncoderData
from Brain.movement import MotorCortex
from Brain.vector import Vector
from Brain.recording import mark_begining
from Brain.plan import MovementStep
from Brain.world import WORLD


pi_2, pi__2, = math.pi / 2.0, - math.pi / 2.0

t_s = 0.0
MAX_STEP = 27.0

def change_state(encoder, movement, world):
	""" simulate encoder readings
	"""
	global t_s
	encoder._process_input( EncoderData(
								movement._sent_left * MAX_STEP, 
								movement._sent_right * MAX_STEP, 
								t_s) 
							)
	world.pos_x, world.pos_y, world.angle, = encoder.position.x, encoder.position.y, encoder.position.angle, 
	t_s += 1 


if __name__ == '__main__':
	world = WORLD()
	encoder = Encoder()
	encoder.setDaemon(True)
	move = MotorCortex(world)
	
	print 'STOPING ..'
	encoder.reset(Vector(0, 0), 0)
	world.pos_x, world.pos_y, world.angle, = 0.0, 0.0, 0.0,
	move.stop()
	
	move.start_task( MovementStep(-1000.0, 0.0) )
	
	index = 0
	for x in range(350):
		move._run_logic()
		change_state(encoder, move, world)
		if len(move.history.data) > index:
			print move.history.data[index]
			index += 1
		else:
			print ' - no new records'

	print 'done.'



